#include "util.h"


/* Function prototypes */

static inline void merge_f(data_t * __restrict A, int p, int q, int r, data_t * __restrict buffer) ;
static inline void copy_f(data_t * __restrict source, data_t * __restrict dest, int n) ;
void sort_f_buffered(data_t * __restrict A, int p, int r, data_t * __restrict buffer) ;

/* Function definitions */

/* Basic merge sort;
   this function sets up buffer then delegates the rest of the
   recursion to the sort function expecting a buffer. */
void sort_f(data_t * A, int p, int r) 
{
	// Create scratch space for merge usage:
	data_t * __restrict buffer = 0 ;
	mem_alloc(&buffer, (p + r) / 2 - p + 2) ;
	if (buffer == NULL)
	{
       	    mem_free(&buffer) ;
       	    return ;
	}

	// Hand off buffer and arguments to the recursive sort function:
	sort_f_buffered(A, p, r, buffer) ;

	// Free the scratch space before completing:
	mem_free(&buffer) ;
}

/* Body of the merge sort. Expects a pre-allocated buffer */
void sort_f_buffered(data_t * __restrict A, int p, int r, data_t * __restrict buffer)
{
	assert (A) ;
	int q;
	if (r - p <= 49)
	{
	    isort(A + p, A + r) ;
	}
	else
	{
	    q = (p + r) / 2 ;
	    sort_f_buffered(A, p, q, buffer) ;
	    sort_f_buffered(A, q + 1, r, buffer) ;
	    merge_f(A, p, q, r, buffer) ;
	}
}

/* A merge routine. Merges the sub-arrays A [p..q] and A [q + 1..r].
 * Uses two arrays 'left' and 'right' in the merge operation.
 * Makes use of a buffer given to it at initialization
 */
static inline void merge_f(data_t * __restrict A, int p, int q, int r, data_t * buffer) 
{ 
	assert(A) ;
	assert(p <= q) ;
	assert((q + 1) <= r) ;
	int n1 = q - p + 1 ;

	data_t * __restrict right = A + q ; 

	copy_f(A + p, buffer, n1) ;
	buffer[n1] = UINT_MAX ;

	data_t * __restrict rp = A + r ;
	data_t * __restrict Ap = A + p ;
	
	data_t cmp;
	data_t min;
	while(Ap <= rp)
	{
	    cmp = *buffer <= *right ;
	    min = *right ^ ((*buffer ^ *right) & (-cmp)) ;
	    *(Ap++) = min ;
	    buffer += cmp ;
	    right += !cmp ;
	}
}

static inline void copy_f(data_t * __restrict source, data_t * __restrict dest, int n)                              
{                                                                             
        assert (dest) ;                                                      
        assert (source) ;
	data_t * __restrict top = dest + n ;	
                                                               
	for ( ; dest < top ; dest++)
	{                                                 
	  *(dest) = *(source++) ;
	}
}
